/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvWorld/trajectory.h
 * @Description  : 定义了与轨迹相关的类和数据结构，包括不同类型的轨迹、投影数据和车辆运动模式。 
 *                 包括直线轨迹、侧移轨迹和旋转轨迹的实现。
 *                 提供了计算车辆进度、速度、曲率等功能的接口。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 15:38:19
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once
#include "geometry.h"
#include <cmath>

constexpr float MAX_DIS_NEAR_POINT = 0.25f;   // 12cm-上线时，距离节点的距离判断是否为节点上上线

enum class TrajType {
    UNDEFINED_TRAJ = -1,
    LINE_TRAJ,   // 0
    BEZIER_TRAJ, // 1
    SIDE_TRAJ,   // 2
    SPIN_TRAJ    // 3
};

enum class TagTrajDrivePattern {
    DRIVE_PATTERN_TURN,
    DRIVE_PATTERN_SHIFT
};

// 原地打舵时通知底盘正反向或自动判断
enum class SteerDirReport {
    SteerDirAuto = 0,
    SteerDirPositie = 1, // 正向
    SteerDirOpposite = 2 // 反向
};

constexpr float PROJECT_DX_FACTOR = 1000.0f;               // 1m equivalent to 1
constexpr float PROJECT_THITA_FACTOR = (57.2958f * 10.0f); // 1 rad equivalent to 10
constexpr float PROJECT_TYPE_FACTOR = 30.0f;               // Outside type equivalent to 30mm

//
//   A structure defines the data of a projection from a posture to a trajectory.
//
class ProjectData
{
 public:
    enum class ProjectionType {
        type_inside = 0,
        type_outside_start,
        type_outside_end
    };

 public:
    ProjectData()
        : type_(ProjectionType::type_inside), x_(0.0f), y_(0.0f), relative_progress_(0.0f),
          tangent_(0.0f), theta_(0.0f), outside_distance_(0.0f), curvature_(0.0f), deviation_progress_(0.0f) {}

    // The copy constructor
    void operator=(const ProjectData& data) {
        type_ = data.type_;
        x_ = data.x_;
        y_ = data.y_;
        relative_progress_ = data.relative_progress_;
        tangent_ = data.tangent_;
        theta_ = data.theta_;
        outside_distance_ = data.outside_distance_;
        curvature_ = data.curvature_;
        deviation_progress_ = data.deviation_progress_;
    }

    float DevFactor() const {
        Angle ang = abs(Angle(theta_));
        float dx_factor = PROJECT_DX_FACTOR * std::fabs(x_);
        float outside_factor = (type_ == ProjectionType::type_inside) ? 0 : PROJECT_THITA_FACTOR * outside_distance_;
        float theta_factor = PROJECT_THITA_FACTOR * ang.radian_;
        return (dx_factor + theta_factor + outside_factor);
    }

    // overloaded operator "<"
    bool operator<(const ProjectData& data) const {
        return DevFactor() < data.DevFactor();
    }

 public:
    ProjectionType type_;                // The projection type : 0-inside, 1-outside(start), 2-outside(end)
    float x_;                            // 左右方向偏差(因为车体笛卡尔坐标系)；符号：运行方向的轨迹的左负右正；The X distance from the posture to the
    float y_;                            // 前后进度(因为车体笛卡尔坐标系);只有在起点之前或终点之后，才会不为0：The Y distance from the posture to the reference point
    float relative_progress_;            // The progress variable
    float tangent_;                      // The tangent angle of the projection point
    float theta_;                        // The angular deviation (can not be negative)
    float outside_distance_;             // The outside distance
    float curvature_;                    // The curature of the curve at the projection point
    float deviation_progress_;           // x方向前后进度误差
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "Trajectory ".
class Trajectory 
{
 public:
    // Default constructor
    Trajectory()
        : type_((int)TrajType::UNDEFINED_TRAJ), steer_angle_(0), shift_angle_(0) {}

    virtual ~Trajectory() = default;

    // Get the type of the trajectory
    short GetType() const { return type_; }

    // Vehicle's drive mode on the trajectory
    virtual short GetDrivePattern() const { return static_cast<short>(TagTrajDrivePattern::DRIVE_PATTERN_TURN); }

    // Get the wheel's move direction
    virtual MoveDir& WheelMoveDir() { return wheel_move_dir_; }

    // Get the vehicle's start steer angle
    virtual Angle& StartSteerAngle() { return steer_angle_; }

    // Get the vehicle's end steer angle
    virtual Angle& EndSteerAngle() { return steer_angle_; }

    // Get the vehicle's start heading angle
    virtual Angle& StartHeading() { return heading_angle_; }

    // Get the vehicle's end heading angle
    virtual Angle& EndHeading() { return heading_angle_; }

    // Get the start point of the trajectory
    virtual Point StartPoint() { return current_point_; }

    // Get the end point of the trajectory
    virtual Point EndPoint() { return current_point_; }

    // Get the progress "from" value
    virtual float GetFrom() { return from_; }

    // Get the progress "to" value
    virtual float GetTo() { return to_; }

    // Get the total range of the progress variable
    virtual float GetRange() { return range_; }

    // Set the progress variable to specify the current point
    virtual void SetRelProgress(float rate, float relative_progress) = 0;

    // Preprocessing the progress variable to specify the current point
    // virtual void PreSetProgress(float fRate, float fPhi){(void)(fRate);(void)(fPhi) ; return;}

    // The trajectory generation function
    virtual Point& TrajFun() { return current_point_; }

    // The heading generation function
    virtual Angle& HeadingFun() { return heading_angle_; }

    // The steer velocity generation function
    virtual Angle& SteeringFun() { return steer_angle_; }

    // The posture generation function
    virtual Posture &PostureFun() {
        current_posture_.SetPosture(current_point_, heading_angle_);
        return current_posture_;
    }

    // Get the shifting angle
    virtual Angle& ShiftAngle() { return shift_angle_; }

    // The velocity generation function
    virtual Velocity& VelocityFun() { return velocity_; }

    // The curvature at the start point
    virtual float CurvatureFun() { return curvature_; }

    // The curvature at the start point
    virtual float StartCurvature() { return 0.0f; }

    // The curvature at the end point
    virtual float EndCurvature() { return 0.0f; }

    // Get the deviation between the trajectory and the specified posture
    virtual bool ProjectPosture(Posture& posture, ProjectData* data, bool init_posture = false) = 0;

    // Get the profile value for the given velocity vector
    // virtual float GetProfileArea(float fLinearArea) { return fLinearArea; }

    // Get the profile value for the given velocity vector
    virtual float GetProfileValue(float linear_vel) { return linear_vel; }

    // Get the profile slope for the given acceleration
    virtual float GetProfileSlope(float acc) { return acc; }

    // Get the linear according to the profile value
    virtual float GetLinearVel(float value) { return value; }

    // Limit the linear vel according to the curvature
    virtual float GetLimitRate(float linear_vel) { return linear_vel; }

    // 用来计算原地打舵时的速度矢量(不代表下一路段的轮转向)
    virtual Velocity VelocityForSteerAngle(const float& virtual_vel, int& steer_dir) {
        (void) virtual_vel;
        steer_dir = (int)SteerDirReport::SteerDirAuto; // 0:auto 1:positive 2:negative
        return Velocity();
    }
    // 用来计算车头方向角角度路径切线角度差(全方位agv纠偏使用)
    virtual Angle HeadToTangent() = 0;

 public:
    short type_;              // Trajectory type #
    float progress_rate_;     // Vel: curCurValue = profiler.setCurValue //The rate of progress
    float from_;              // The position to start from
    float to_;                // The position to end at
    float range_;             // The range of the trajectory specification
    MoveDir wheel_move_dir_;  // Wheels' move direction(FWD/BWD)
    Angle heading_angle_;     // Tangent angle at the current point
    float curvature_;         // Curvature at the current point
    Angle steer_angle_;       // Steer angle maybe can be reverse
    Angle shift_angle_;       // Shift angle ln.slantAngle - angleHead(side scp sppshift);init set once
    Point current_point_;     // Vehicle current point
    Posture current_posture_; // Vehicle posture at the current point
    Velocity velocity_;       // Vehicle velocity at the current point
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "LineTraj".
class LineTrajectory : public Trajectory 
{
 public:
    LineTrajectory() = default;
    virtual ~LineTrajectory() override = default;

    // Trajectory creation function
    void CreateTrajectory(const Point& start_point, const Point& end_point,
        MoveDir wheel_move_dir, float from = 0, float to = -1.0f);

    // Get the start point of the trajectory
    Point StartPoint() override { return line_.start_point_; }

    // Get the end point of the trajectory
    Point EndPoint() override { return line_.end_point_; }

    // Set the progress variable to specify the current point
    void SetRelProgress(float rate, float current_length) override;

    // Preprocessing the progress variable to specify the current point
    // virtual void PreSetProgress(float fRate, float fPhi);

    // The velocity generation function
    Velocity &VelocityFun() override;

    // Get the deviation between the trajectory and the specified posture
    bool ProjectPosture(Posture& posture, ProjectData* data, bool init_posture = false) override;
    
    // Get the total range of the progress variable
    float GetRange() override;
    
    // 用来计算原地打舵时的速度矢量
    Velocity VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) override;

    // 用来计算车头方向角角度路径切线角度差
    Angle HeadToTangent() override;
 private:
    Line line_; // The line object
};

// 1
//////////////////////////////////////////////////////////////////////////////
//   The interface of class "TSideTraj".
class SideTrajectory : public Trajectory 
{
 public:
    // The constructor
    SideTrajectory() = default;
    ~SideTrajectory() override = default;

    // Create the SIDE trajectory
    void CreateTrajectory(const Point& start_point, const Point& end_point,
        const Angle& heading_angle, float from = 0, float to = -1.0f);

    // Vehicle's drive mode on the trajectory
    short GetDrivePattern() const override { return static_cast<short>(TagTrajDrivePattern::DRIVE_PATTERN_SHIFT); }

    // Get the start point of the trajectory
    Point StartPoint() override { return current_point_; }

    // Get the end point of the trajectory
    Point EndPoint() override { return current_point_; }

    // Set the progress variable to specify the current point
    void SetRelProgress(float rate, float current_length) override;

    // The velocity generation function
    Velocity& VelocityFun() override;

    // 用来计算原地打舵时的速度矢量
    Velocity VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) override;

    // 用来计算车头方向角角度路径切线角度差
    Angle HeadToTangent() override;

    // Get the deviation between the trajectory and the specified posture
    // virtual BOOL ProjectPosture(CPosture& pst, CProjectData* pData, BOOL bDynTransPos = FALSE);// {return FALSE;}

    // Get the deviation between the trajectory and the specified posture
    bool ProjectPosture(Posture& posture, ProjectData* data, bool init_posture = false) override;

    // Get the side-move direction (Forward=right, backward=left)
    MoveDir GetSideMoveDir() const { return side_move_dir_; }

    // The curvature at the start point
    float StartCurvature() override { return steer_angle_.radian_; }

    // The curvature at the end point
    float EndCurvature() override { return steer_angle_.radian_; }

    // 检查是否可以反转转向
    bool CanSteerReserve();

    // 反转转向
    void ReserveSteer();

    // 设置转向方向
    void SetWheelDir();

 private:
    Line line_;               // 线对象
    MoveDir side_move_dir_;   // 侧向移动方向
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "TSpinTraj".
class SpinTrajectory : public Trajectory 
{
 public:
    // The constructor
    SpinTrajectory() = default;
    ~SpinTrajectory() override = default;

    // 只有自旋检测地标时，才会调用此函数
    // Trajectory creation function
    // void CreateTraj(CPnt &pt, CAngle &angStart, CAngle &angEnd, CTurnDir TurnDir, float fSpinR,
    //                 float fFrom = 0, float fTo = -1.0f);

    // 正常自旋逻辑，调用此函数
    // Trajectory creation function
    void CreateTrajectory(const Point& point, const Angle& start_angle, const Angle& end_angle,
        TurnDir turn_direction, float from = 0, float to = -1.0f);

    // Get the vehicle's start heading angle
    Angle& StartHeading() override { return start_heading_angle_; }

    // Get the vehicle's end heading angle
    Angle& EndHeading() override { return end_heading_angle_; }

    // Get the start point of the trajectory
    Point StartPoint() override { return current_point_; }

    // Get the end point of the trajectory
    Point EndPoint() override { return current_point_; }

    // Get the total range of the progress variable
    float GetRange() override { return turn_angle_; }

    // Set the progress variable to specify the current point
    void SetRelProgress(float rate, float phi) override;

    // The velocity generation function
    Velocity& VelocityFun() override;

    // The curvature at the start point
    float StartCurvature() override { return curvature_; }

    // The curvature at the end point
    float EndCurvature() override { return curvature_; }

    Velocity VelocityForSteerAngle(const float& virtual_velocity, int& steer_direction) override;

    // 用来计算车头方向角角度路径切线角度差
    Angle HeadToTangent() override;

    // Get the deviation between the trajectory and the specified posture
    bool ProjectPosture(Posture& posture, ProjectData* data, bool init_posture = false) override;

    // Get the profile value for the given velocity vector
    // virtual float GetProfileArea(float fAngularArea) override;

    // Get the profile value for the given velocity vector
    float GetProfileValue(float angle_velocity) override { return angle_velocity; }

    // Get the profile slope for the given acceleration
    float GetProfileSlope(float acc) override { return acc; }

    // 自旋旋转的方向
    TurnDir GetTurnDir() const { return turn_direction_; }

    // 是否时同步自旋，此标识废弃
    // BOOL IsSyncType() { return m_bSyncType; }
   
    // 根据当前反馈的实际角度(单位：弧度) 修改自旋方向
    bool ModifySpinDir(const Angle& start_angle);
 private:
    Angle start_heading_angle_;  // 车辆的起始朝向
    Angle end_heading_angle_;    // 车辆的结束朝向
    float turn_angle_;           // 轨迹的旋转角度
    float curvature_;            // 曲率
    TurnDir turn_direction_;     // 旋转方向
};

